package naito_rescue.router;

import rescuecore2.misc.*;
import rescuecore2.misc.geometry.*;
import rescuecore2.worldmodel.*;
import rescuecore2.standard.entities.*;

import naito_rescue.*;
import naito_rescue.agent.*;
import naito_rescue.router.object.*;
import naito_rescue.object.*;
import java.util.*;
import java.util.Map.*;

public abstract class AbstractRoutingModule
{
	protected NAITOAgent            owner;
	protected StandardWorldModel    model;
	protected ConnectivityGraph     graph;
	protected PassableChecker       checker;
	protected MyLogger              logger;
	protected boolean               isPF;
	
	public AbstractRoutingModule(NAITOAgent owner, ConnectivityGraph graph){
		this.owner = owner;
		this.model = owner.getWorldModel();
		this.graph = graph;
		
		this.checker = new PassableChecker(owner);
		this.logger = owner.getLogger();
		this.isPF = (owner instanceof NAITOPoliceForce);
	}
    public int computePathLength(List<EntityID> path)
    {
        if (path != null)
        {
            if (path.size() >= 2)
            {
                EntityID previous;
                EntityID current;
                int pathLength = 0;
                Iterator<EntityID> it = path.iterator();
                current = it.next();
                while (it.hasNext())
                {
                    previous = current;
                    current = it.next();
                    pathLength += model.getDistance(model.getEntity(previous), model.getEntity(current));
                }
                return pathLength;
            }
        }
        return 0;
    }
    protected boolean isGoal(EntityID e, Collection<EntityID> test) {
        for (EntityID next : test) {
            if (next.equals(e)) {
                return true;
            }
        }
        return false;
    }
    
    //このあたりはIAMRescueあたりに実装があるはず
    protected static final int MOVEABLE_LENGTH_AT_ONCE = 30000;
    protected boolean shouldCutOffSearching(List<EntityID> path){
    	if(path == null || path.isEmpty()){
    		return false;
    	}
    	int cost = computePathLength(path);
    	if(cost > MOVEABLE_LENGTH_AT_ONCE){
    		return true;
    	}else{
    		return false;
    	}
    }
    
    protected SearchNode getNodeFromOpenList(PriorityQueue<SearchNode> open, EntityID id){
    	for(SearchNode node : open){
    		if(node.getNodeID().getValue() == id.getValue()){
    			return node;
    		}
    	}
    	return null;
    }
    
    protected List<EntityID> cutPath(List<EntityID> path){
    	String context = "AbstractRoutingModule#cutPath";
        if (path != null)
        {
            if (path.size() >= 2)
            {
                EntityID previous;
                EntityID current;
                List<EntityID> newPath = new ArrayList<EntityID>();
                int pathLength = 0;
                Iterator<EntityID> it = path.iterator();
                current = it.next();
                newPath.add(current);
                while (it.hasNext())
                {
                    previous = current;
                    current = it.next();
                    pathLength += model.getDistance(model.getEntity(previous), model.getEntity(current));
                    if(pathLength < MOVEABLE_LENGTH_AT_ONCE){
						newPath.add(current);
					}else{
						//logger.debug("Old path = [" + path + "]", context);
						//logger.debug("New path = [" + newPath + "]", context);
						return newPath;
					}
                }
				//logger.debug("Old path = [" + path + "]", context);
				//logger.debug("New path = [" + newPath + "]", context);
                return newPath;
            }
        }
        return null;
    }
}
